%
%
%
clear all
close all
clc

Maxx            = 100;
Maxy            = 100;
N               = 90;
% SnPos           = zeros(100,100,2);
% for i = 1:100
%     SnPosStore(i,:,:)        = round(100*rand(N,2));
% end 
% save('SnPosStoreFile100.mat','SnPosStore');

load('SnPosStoreFile100.mat');
CommDist            = 20;
BeaconDeltad        = 5;
ThetaSlope          = pi*40/180;
Dx                  = (BeaconDeltad*cos(ThetaSlope));
Dy                  = (BeaconDeltad*sin(ThetaSlope));
Nstops              = 1:20;
BeaconPosStore      = [(Dx*Nstops)' (Dy*Nstops)' ];
NodePosStore        = reshape(SnPosStore(68,:,:),N,2);
figure(100);
plot(NodePosStore(:,1),NodePosStore(:,2),'rx'); hold on;
plot(BeaconPosStore(:,1),BeaconPosStore(:,2),'bx');
plot(BeaconPosStore(:,1),BeaconPosStore(:,2),'b');

BeaconPos           = [0,0];
BeaconPacket        = struct('PacketHeader',1001,'Posx',Maxx/2,'Posy',Maxy/2);
NodePacket          = struct('PacketHeader',5001,'Posx',Maxx/2,'Posy',Maxy/2);


% NODEID, POSX, POSY, . ASSUMING THAT ALL NODES ARE AT CENTER. 
NodeBeaconInfo = struct();
NeighbourIdx   = [];
for ii = 1:length(NodePosStore)
    NodeInfo(1,ii) = struct('NodeID',5000+ii,'Posx',Maxx/2,'Posy',Maxy/2,'NumBeacons',0,'BeaconInfo',NodeBeaconInfo,'NeighbourIdx',NeighbourIdx,'Actualx',NodePosStore(ii,1),'Actualy',NodePosStore(ii,2)); 
end
for ii = 1:length(NodePosStore)
    CurrNodeID = NodeInfo(1,ii).NodeID;
    NeighbourIdx = [];
    for jj = 1:length(NodePosStore)
        if(CurrNodeID ~= NodeInfo(1,jj).NodeID)
            err = sqrt(sum((NodePosStore(ii,:) - NodePosStore(jj,:)).^2)) - CommDist;
            if err < 0
                NeighbourIdx = [NeighbourIdx jj];
            end
        end
    end
    NodeInfo(1,ii).NeighbourIdx = NeighbourIdx;
end

UnCoveredMap     = ones(Maxx,Maxy);
MapMask          = zeros(Maxx,Maxy);
MaxIter          = 100;
NextTheta        = pi*40/180;
BeaconPos        = [ 0 0];
BeaconPosStore   = zeros(MaxIter,2);
for iteration = 1:MaxIter
    ThetaSlope   = NextTheta;
    Dx           = (BeaconDeltad*cos(ThetaSlope));
    Dy           = (BeaconDeltad*sin(ThetaSlope));
    BeaconPos    = BeaconPos + [Dx Dy];%BeaconPosStore(ii,:);
    if BeaconPos(1,1) < 0% x
        
        Deltax = centroidx - BeaconPos(1,1);
        Deltay = centroidy - BeaconPos(1,2);
        NextTheta =  find_thetar(Deltax,Deltay);
    elseif BeaconPos(1,2) < 0% y
        Deltax = centroidx - BeaconPos(1,1);
        Deltay = centroidy - BeaconPos(1,2);
        NextTheta =  find_thetar(Deltax,Deltay);
    elseif BeaconPos(1,1) > Maxx% x
        Deltax = centroidx - BeaconPos(1,1);
        Deltay = centroidy - BeaconPos(1,2);
        NextTheta =  find_thetar(Deltax,Deltay);
    elseif BeaconPos(1,2) > Maxy% y
        Deltax = centroidx - BeaconPos(1,1);
        Deltay = centroidy - BeaconPos(1,2);
        NextTheta =  find_thetar(Deltax,Deltay);
    else
        for iix = 1:Maxx
            for iiy = 1:Maxy
                Dist = sqrt((iix - BeaconPos(1,1)).^2  + (iiy - BeaconPos(1,2)).^2) - CommDist;
                if Dist < 0 
                    MapMask(iix,iiy) = 0;
                else
                    MapMask(iix,iiy) = 1;
                end
            end
        end
                
        UnCoveredMap = UnCoveredMap .*MapMask;
        IDX     = find(UnCoveredMap == 1);
        [I,J]   = ind2sub(size(UnCoveredMap),IDX);
        centroidx = mean(I);
        centroidy = mean(J);
    end
    

    Diff         = NodePosStore - repmat(BeaconPos,N,1);
    Mask         = (sqrt(sum(Diff.^2,2)) - repmat(CommDist,N,1)) < 0;
    ValidNode    = NodePosStore(Mask,:);
    ValidNodeIdx = find(Mask==1);
    
    th          = linspace(0,2*pi,200)';
    x1e         = repmat(CommDist*cos(th),size(ValidNode,1),1) + repmat(ValidNode(:,1),200,1); 
    y1e         = repmat(CommDist*sin(th),size(ValidNode,1),1) + repmat(ValidNode(:,2),200,1);
    BeaconPacket.Posx    = BeaconPos(1,1);
    BeaconPacket.Posy    = BeaconPos(1,2);
    
    BeaconPosStore( iteration,:)  = [BeaconPacket.Posx BeaconPacket.Posy];

    
    figure(100);
    plot(ValidNode(:,1),ValidNode(:,2),'go');
    %plot(x1e,y1e,'cx');
    plot(BeaconPacket.Posx,BeaconPacket.Posy,'bo');
    axis equal;
    

    for currentidx = 1:length(ValidNodeIdx)
         jj                         = ValidNodeIdx(currentidx);
         NodeInfo(1,jj).NumBeacons  = NodeInfo(1,jj).NumBeacons +1;
         numbeacon                  = NodeInfo(1,jj).NumBeacons ;
         NodePosx                   = NodePosStore(jj,1);
         NodePosy                   = NodePosStore(jj,2);
         NodeInfo(1,jj).BeaconInfo(numbeacon).Posx = BeaconPacket.Posx;
         NodeInfo(1,jj).BeaconInfo(numbeacon).Posy = BeaconPacket.Posy;
         Deltax = NodePosx - BeaconPacket.Posx ;
         Deltay = NodePosy - BeaconPacket.Posy ; 
         NodeInfo(1,jj).BeaconInfo(numbeacon).R    = sqrt((Deltax).^2+( Deltay).^2);
         % Theta
                
         thetar  = find_thetar(Deltax,Deltay);
         
         NodeInfo(1,jj).BeaconInfo(numbeacon).thetar = thetar;
         
         if(numbeacon >= 3)
             x1 = NodeInfo(1,jj).BeaconInfo(1).Posx ; y1 = NodeInfo(1,jj).BeaconInfo(1).Posy ; r1 = NodeInfo(1,jj).BeaconInfo(1).R; theta1 = NodeInfo(1,jj).BeaconInfo(1).thetar;
             x2 = NodeInfo(1,jj).BeaconInfo(2).Posx ; y2 = NodeInfo(1,jj).BeaconInfo(2).Posy ; r2 = NodeInfo(1,jj).BeaconInfo(2).R; theta2 = NodeInfo(1,jj).BeaconInfo(1).thetar;
             x3 = NodeInfo(1,jj).BeaconInfo(3).Posx ; y3 = NodeInfo(1,jj).BeaconInfo(3).Posy ; r3 = NodeInfo(1,jj).BeaconInfo(3).R; theta3 = NodeInfo(1,jj).BeaconInfo(1).thetar;
             A  = 2*[x3 - x1 , y3 - y1; ...
                   x3 - x2 , y3 - y2; ...  
                  ];

             b = [(r1^2 - r3^2 )-(x1^2 - x3^2)-(y1^2 - y3^2);
                 (r2^2 - r3^2 )-(x2^2 - x3^2)-(y2^2 - y3^2);
                 ];
             %xyest = inv(A'*A)*A'*b;
             xest = mean(r1*cos(theta1) + r2*cos(theta2) + r3*cos(theta3));
             yest = mean(r1*sin(theta1) + r2*sin(theta2) + r3*sin(theta3));
             NodeInfo(1,jj).settled    = 1;
             err   = [xest;yest] - [NodePosx ; NodePosy];
             % fprintf('%d th node is settled with error Errx = %d,Erry = %d\n',jj,err);
         else 
             NodeInfo(1,jj).settled    = 0;
         end
    end
    % ALGORITHM 1
    S            = struct();
    MBP          = struct();
    MBP.Neighidx = ValidNodeIdx;
    MBP.Table    = zeros(size(ValidNodeIdx,1),6); % ID,Group,Settled,TotalSettled,TotalUnSettled,AN
    for MBPNidx  = 1:length( MBP.Neighidx)
        MBPNnode                   = MBP.Neighidx(MBPNidx);
        MBP.Table(MBPNidx,1)       = MBPNnode ;
        MBP.Table(MBPNidx,3)       = NodeInfo(1,MBPNnode).settled; 
        if(NodeInfo(1,MBPNnode).settled == 1)
            NodePosx    = NodePosStore(MBPNnode,1);
            NodePosy    = NodePosStore(MBPNnode,2);
            Deltax      = NodePosx - BeaconPacket.Posx ;
            Deltay      = NodePosy - BeaconPacket.Posy ; 
            thetar      = find_thetar(Deltax,Deltay);
            S.Neighidx  = NodeInfo(1,MBPNnode).NeighbourIdx; % Neighbours of MBP neighbours
            Settled     = 0;
            UnSettled   = 0;
            for SNidx = 1:length(S.Neighidx)
                SNnode = S.Neighidx(SNidx);
                for MBPNidx2 = 1:length( MBP.Neighidx)
                    if( SNnode ~= MBP.Neighidx(MBPNidx2) ) % making sure Neighbours of MBP neighbours are not MBP neighbours
                        if(NodeInfo(1,SNnode).settled == 1)
                            Settled = Settled +1;          % settled neighbours, neighbour
                        else
                            UnSettled = UnSettled +1;      % unsettled neighbours, neighbour
                        end
                    end
                end
            end
            MBP.Table(MBPNidx,4)        = Settled;
            MBP.Table(MBPNidx,5)        = UnSettled;
            MBP.Table(MBPNidx,6)        = thetar;
        else
            MBP.Table(MBPNidx,4)        = 0;
            MBP.Table(MBPNidx,5)        = 0;
        end
    end

    UnsettleList = MBP.Table(:,5).*MBP.Table(:,3);
    if(sum(UnsettleList) ~= 0)
        [Y,I]        = max(UnsettleList);
        I            = find(UnsettleList == Y);
        if ( length(I) > 1)
            SettleList = MBP.Table(I,4);
            [Y,Ind]    = min(SettleList);
            I          = I(Ind);
        end
        NextTheta = MBP.Table(I,6);
        fprintf('New Theta = %f degrees\n',MBP.Table(I,6)*180/pi);
%     else
%         SettleList = MBP.Table(:,4).*MBP.Table(:,3);
%         if(sum(SettleList) ~=0)
%             [Y,I]        = max(UnsettleList);
%             NextTheta = MBP.Table(I,6) ; % Go in opposite direction
%         end
    end
    

end




%             case 0
%                 NodePacket(jj).B1Posx       = BeaconPacket.Posx; 
%                 NodePacket(jj).B1Posy       = BeaconPacket.Posy;
%                 NodePacket(jj).NumBeacons   = 1;
%             case 1
%                 NodePacket(jj).B2Posx = BeaconPacket.Posx; 
%                 NodePacket(jj).B2Posy = BeaconPacket.Posy; 
%                 NodePacket(jj).NumBeacons   = 2;
%             case 2
%                 NodePacket(jj).B3Posx = BeaconPacket.Posx; 
%                 NodePacket(jj).B3Posy = BeaconPacket.Posy; 
%                 NodePacket(jj).NumBeacons   = 3;
%                 
%             case 3
%                 NodePacket(jj).B2Posx = BeaconPacket.Posx; 
%                 NodePacket(jj).B2Posy = BeaconPacket.Posy; 
%                 NodePacket(jj).NumBeacons   = 2;
%             otherwise











